#include "ragdoll.h"





Ragdoll::Ragdoll(const NxVec3& pos)
{
	
}

void Physixer::initRagdoll()
{
	NxQuat qRotLeft, qRotRight, qRotAround;
	qRotLeft.fromAngleAxis(90, NxVec3(0,0,1));
	qRotRight.fromAngleAxis(-90, NxVec3(0,0,1));
	qRotAround.fromAngleAxis(180, NxVec3(0,0,1));

	NxMat33 mRotLeft, mRotRight, mRotAround;
	mRotLeft.fromQuat(qRotLeft);
	mRotRight.fromQuat(qRotRight);
	mRotAround.fromQuat(qRotAround);

	// Create body parts
	guy->head = CreateSphere(NxVec3(0,32.8,0), 2.38, 10);
	guy->torso = CreateCapsule(NxVec3(0,26.9,0), 8.238 - 7.073, 7.073/2, 10);
	guy->pelvis = CreateSphere(NxVec3(0,19.85,0), 3, 10);

	guy->leftUpperArm = CreateCapsule(NxVec3(-6.601,27.793,0), 5.69, 2.27/2, 10);
	guy->leftUpperArm->setGlobalOrientationQuat(qRotRight);
	guy->leftForeArm = CreateCapsule(NxVec3(-11.163,28.054,0), 4.596, 3.0/2, 10);
	guy->leftForeArm->setGlobalOrientationQuat(qRotRight);
	guy->leftHand = CreateBox(NxVec3(-15.462,27.487,0), NxVec3(1,1.5,1.5), 10);
	guy->leftHand->setGlobalOrientationQuat(qRotRight);

	guy->rightUpperArm = CreateCapsule(NxVec3(6.601,27.793,0), 5.69, 2.27/2, 10);
	guy->rightUpperArm->setGlobalOrientationQuat(qRotLeft);
	guy->rightForeArm = CreateCapsule(NxVec3(11.163,28.054,0), 4.596, 3.0/2, 10);
	guy->rightForeArm->setGlobalOrientationQuat(qRotLeft);
	guy->rightHand = CreateBox(NxVec3(15.462,27.487,0), NxVec3(1,1.5,1.5), 10);
	guy->rightHand->setGlobalOrientationQuat(qRotLeft);

	guy->leftThigh = CreateCapsule(NxVec3(-2.263,14.144,0), 8.9-3.2, 3.2/2, 10);
	//guy->leftThigh->setGlobalOrientationQuat(qRotAround);
	guy->leftCalf = CreateCapsule(NxVec3(-2.322,8.1,0), 9.2-3.9, 3.9/2, 10);
	//guy->leftCalf->setGlobalOrientationQuat(qRotAround);
	guy->leftFoot = CreateBox(NxVec3(-2.427,2,-0.905), NxVec3(4.413/2,2.707/2,6.819/2), 10);
	//guy->leftFoot->setGlobalOrientationQuat(qRotAround);

	guy->rightThigh = CreateCapsule(NxVec3(2.263,14.144,0), 8.9-3.2, 3.2/2, 10);
	//guy->rightThigh->setGlobalOrientationQuat(qRotAround);
	guy->rightCalf = CreateCapsule(NxVec3(2.322,8.171,0), 9.2-3.9, 3.9/2, 10);
	//guy->rightCalf->setGlobalOrientationQuat(qRotAround);
	guy->rightFoot = CreateBox(NxVec3(2.427,2,-0.905), NxVec3(4.413/2,2.707/2,6.819/2), 10);
	//guy->rightFoot->setGlobalOrientationQuat(qRotAround);


	guy->neck = CreateBodySphericalJoint(guy->head,guy->torso,NxVec3(0,31,0),NxVec3(0,1,0));
	guy->leftShoulder = CreateBodySphericalJoint(guy->leftUpperArm,guy->torso,NxVec3(-4.891,28.057,0),NxVec3(1,0,0));
	guy->rightShoulder = CreateBodySphericalJoint(guy->rightUpperArm,guy->torso,NxVec3(4.891,28.057,0),NxVec3(-1,0,0));
	guy->spine = CreateBodySphericalJoint(guy->torso,guy->pelvis,NxVec3(0,22.148,0),NxVec3(0,-1,0));
	guy->leftHip = CreateBodySphericalJoint(guy->leftThigh,guy->pelvis,NxVec3(-2.152,18.654,0),NxVec3(0,-1,0));
	guy->rightHip = CreateBodySphericalJoint(guy->rightThigh,guy->pelvis,NxVec3(2.152,18.654,0),NxVec3(0,-1,0));

	guy->leftElbow = CreateRevoluteJoint(guy->leftForeArm,guy->leftUpperArm,NxVec3(-9.011,27.95,0),NxVec3(0,0,-1));
	guy->rightElbow = CreateRevoluteJoint(guy->rightForeArm,guy->rightUpperArm,NxVec3(9.011,27.95,0),NxVec3(0,0,-1));

	guy->leftWrist = CreateBodySphericalJoint(guy->leftHand,guy->leftForeArm,NxVec3(-14.042,27.851,0),NxVec3(0,1,0));
	guy->rightWrist = CreateBodySphericalJoint(guy->rightHand,guy->rightForeArm,NxVec3(14.042,27.851,0),NxVec3(0,1,0));

	guy->leftKnee = CreateRevoluteJoint(guy->leftCalf,guy->leftThigh,NxVec3(-2.4,11.637,0),NxVec3(1,0,0));
	guy->rightKnee = CreateRevoluteJoint(guy->rightCalf,guy->rightThigh,NxVec3(2.4,11.637,0),NxVec3(-1,0,0));

	guy->leftAnkle = CreateRevoluteJoint(guy->leftFoot,guy->leftCalf,NxVec3(-2.379,3.799,0),NxVec3(1,0,0));
	guy->rightAnkle = CreateRevoluteJoint(guy->rightFoot,guy->rightCalf,NxVec3(2.379,3.799,0),NxVec3(-1,0,0));


	//CREATE LIMITS
	NxJointLimitPairDesc limitDesc;

	limitDesc.high.value = (NxReal)0.0*NxPi;
	limitDesc.low.value = -(NxReal)0.5*NxPi;
	guy->leftElbow->setLimits(limitDesc);

	limitDesc.high.value = (NxReal)0.5*NxPi;
	limitDesc.low.value = (NxReal)0.0*NxPi;
	guy->rightElbow->setLimits(limitDesc);

	limitDesc.high.value = (NxReal)0.2*NxPi;
	limitDesc.low.value = -(NxReal)0.2*NxPi;
	guy->leftAnkle->setLimits(limitDesc);

	limitDesc.high.value = (NxReal)0.2*NxPi;
	limitDesc.low.value = -(NxReal)0.2*NxPi;
	guy->rightAnkle->setLimits(limitDesc);

	limitDesc.high.value = (NxReal)0.0*NxPi;
	limitDesc.low.value = -(NxReal)0.5*NxPi;
	guy->leftKnee->setLimits(limitDesc);

	limitDesc.high.value = (NxReal)0.5*NxPi;
	limitDesc.low.value = (NxReal)0.0*NxPi;
	guy->rightKnee->setLimits(limitDesc);

	//DAMPING
	//na razie nic tutaj ;p
}



Physixer::Physixer()
{
	InitNx();
}

void Physixer::InitNx()
{
	// Create the physics SDK
	gPhysicsSDK = NxCreatePhysicsSDK(NX_PHYSICS_SDK_VERSION);
	if (!gPhysicsSDK)  return;

	// Set the physics parameters
	gPhysicsSDK->setParameter(NX_SKIN_WIDTH, 0.01);

	gPhysicsSDK->getFoundationSDK().getRemoteDebugger()->connect ("localhost", 5425);

	// Set the debug visualization parameters
	gPhysicsSDK->setParameter(NX_VISUALIZATION_SCALE, 1);
	gPhysicsSDK->setParameter(NX_VISUALIZE_COLLISION_SHAPES, 1);
	gPhysicsSDK->setParameter(NX_VISUALIZE_ACTOR_AXES, 1);
	gPhysicsSDK->setParameter(NX_VISUALIZE_JOINT_LIMITS, 1);

	// Create the scene
	NxSceneDesc sceneDesc;
	sceneDesc.gravity               = NxVec3(0,-9.8,0);//gDefaultGravity;
	sceneDesc.simType				= NX_SIMULATION_HW;
	//sceneDesc.flags &= NX_SF_SIMULATE_SEPARATE_THREAD;

	gScene = gPhysicsSDK->createScene(sceneDesc);	
	if(!gScene){ 
		sceneDesc.simType				= NX_SIMULATION_SW; 
		gScene = gPhysicsSDK->createScene(sceneDesc);  
		if(!gScene) return;
	}

	// Create the default material
	NxMaterial* defaultMaterial = gScene->getMaterialFromIndex(0); 
	defaultMaterial->setRestitution(0.5);
	defaultMaterial->setStaticFriction(0.5);
	defaultMaterial->setDynamicFriction(0.5);

	// Create the objects in the scene
	CreateGroundPlane();

	guy = new Ragdoll(NxVec3(0,0,0));
	initRagdoll();

	gSelectedActor = guy->torso;
	gForceStrength = 200000;

	gKinematicVelocity = 0.1;
	gDeltaTime = 1.0/60.0;

	// Get the current time
	//previousTime = timeGetTime();
	//UpdateTime();

	// Start the first frame of the simulation
	//if (gScene)  StartPhysics();
}

bool Physixer::IsSelectable(NxActor* actor)
{
   NxShape*const* shapes = gSelectedActor->getShapes();
   NxU32 nShapes = gSelectedActor->getNbShapes();
   while (nShapes--)
   {
       if (shapes[nShapes]->getFlag(NX_TRIGGER_ENABLE)) 
       {           
           return false;
       }
   }

   if (actor == groundPlane)
       return false;

   return true;
}

void Physixer::SelectNextActor()
{
   NxU32 nbActors = gScene->getNbActors();
   NxActor** actors = gScene->getActors();
   for (NxU32 i = 0; i < nbActors; i++)
   {
       if (actors[i] == gSelectedActor)
       {
           NxU32 j = 1;
           gSelectedActor = actors[(i+j)%nbActors];
           while (!IsSelectable(gSelectedActor))
           {
               j++;
               gSelectedActor = actors[(i+j)%nbActors];
           }
           break;
       }
   }
}

void Physixer::SelectNextShapeOnActor(NxActor* actor)
{
	NxU32 nbShapes = actor->getNbShapes();
	NxShape*const* shapes = actor->getShapes();
	for (NxU32 i = 0; i < nbShapes; i++)
	{
		if (shapes[i] == gSelectedShape)
		{
			gSelectedShape = shapes[(i+1)%nbShapes];
			break;
		}
	}
}

void Physixer::RotateActor(NxActor* actor, D3DXVECTOR3* rot)
{
	float _x = rot->x;
	float _y = rot->y;
	float _z = rot->z;

	float length;
	D3DXMATRIX matrix, t;

	memset(matrix, '\0', sizeof(matrix));

	D3DXMatrixIdentity(&matrix);

	// Make sure acceleration value is big enough.
	length = sqrtf(_x * _x + _y * _y + _z * _z);

	D3DXMatrixLookAtRH(&matrix, &D3DXVECTOR3(0,0,0), &D3DXVECTOR3(_x,_z,_y), &D3DXVECTOR3(0,1,0));


	NxMat34 final;
	final.setColumnMajor44((float*)matrix);
	actor->moveGlobalOrientation(final.M);

}

NxVec3 Physixer::ApplyForceToActor(NxActor* actor, const NxVec3& forceDir, const NxReal forceStrength, bool forceMode)
{
	NxVec3 forceVec = forceStrength*forceDir*gDeltaTime;

	if (forceMode)
		actor->addForce(forceVec);
	else 
		actor->addTorque(forceVec);

	return forceVec;
}

NxVec3 Physixer::HoverActor(NxActor* actor)
{
	NxVec3 dir(0,30,0);
	NxVec3 forceVec = dir;//*gDeltaTime;

	actor->addForce(forceVec, NX_ACCELERATION, true);

	return forceVec;
}

NxVec3 Physixer::ApplyForceToActorAtShape(NxActor* actor, NxShape* shape, const NxVec3& forceDir, const NxReal forceStrength, bool forceMode, bool shapeSelectMode)
{
	NxVec3 forceVec = forceStrength*forceDir*gDeltaTime;

	if (forceMode) 
	{
		if (shapeSelectMode) 
		{
			actor->addForceAtLocalPos(forceVec, shape->getLocalPosition());
		} 
		else 
		{
			actor->addForce(forceVec);
		}
	} 
	else 
	{ 
		actor->addTorque(forceVec);
	}

	return forceVec;
}


NxVec3 Physixer::ApplyVelocityToActor(NxActor* actor, const NxVec3& velDir, const NxReal velStrength, bool velMode, const NxReal velScale)
{
	NxVec3 velVec = velStrength*velDir*velScale;

	if (velMode) 
	{
		actor->moveGlobalPosition(actor->getGlobalPosition() + 15*velDir*gDeltaTime);
	} 
	else 
	{
		NxMat33 orient = actor->getGlobalOrientation();
        NxMat33 m;

		m.id();

		NxReal rotSpeed = 10*gDeltaTime;

		if (velDir == NxVec3(1,0,0))
           m.rotX(rotSpeed);
		else if (velDir == NxVec3(-1,0,0))
           m.rotX(-rotSpeed);
		else if (velDir == NxVec3(0,1,0))
           m.rotY(rotSpeed);
		else if (velDir == NxVec3(0,-1,0))
           m.rotY(-rotSpeed);
		else if (velDir == NxVec3(0,0,1))
           m.rotZ(rotSpeed);
		else if (velDir == NxVec3(0,0,-1))
           m.rotZ(-rotSpeed);

		orient = m * orient;

		actor->moveGlobalOrientation(orient);
	}

	return velVec;
}

NxVec3 Physixer::MoveActor(NxActor* actor, D3DXVECTOR3 *mov, D3DXVECTOR3 *grav)
{
	/*
	if (actor->readBodyFlag(NX_BF_KINEMATIC))
		return ApplyVelocityToActor(actor, moveDir, moveStrength, moveMode, 1.0);
	else
		return ApplyForceToActor(actor, moveDir, moveStrength, moveMode);
	*/

	float len = D3DXVec3Length(&(*mov-*grav));

	if (len < 0.1)
		return NxVec3(0,0,0);

	float _x = grav->x;
	float _y = grav->y;
	float _z = grav->z;

	float length;
	D3DXMATRIX matrix, t;

	memset(matrix, '\0', sizeof(matrix));

	D3DXMatrixIdentity(&matrix);

	// Make sure acceleration value is big enough.
	length = sqrtf(_x * _x + _y * _y + _z * _z);

	D3DXMatrixLookAtRH(&matrix, &D3DXVECTOR3(0,0,0), &D3DXVECTOR3(_x,_z,_y), &D3DXVECTOR3(0,1,0));

	D3DXVECTOR3 final;
	//final.x = mov->x;
	//final.y = mov->y;
	//final.z = mov->z;

	//final *= matrix;
	D3DXVec3TransformCoord(&final,&(*mov-*grav),&matrix);

	float moveStrength = D3DXVec3Length(&final);

	NxVec3 moveDir(final.x/moveStrength, final.y/moveStrength, final.z/moveStrength);

	return ApplyVelocityToActor(actor, moveDir, moveStrength, true, 1.0);
}

void Physixer::ReleaseNx()
{
    if (gScene)
	{
		//GetPhysicsResults();  // Make sure to fetchResults() before shutting down
		gPhysicsSDK->releaseScene(*gScene);
	}
	if (gPhysicsSDK)  gPhysicsSDK->release();
    NX_DELETE_SINGLE(gAllocator);
}

void Physixer::ResetNx()
{
	ReleaseNx();
	InitNx();
}

void Physixer::StartPhysics()
{
	// Update the time step
	gDeltaTime = UpdateTime();

	// Start collision and dynamics for delta time since the last frame
    gScene->simulate(gDeltaTime);
	gScene->flushStream();
}


void Physixer::GetPhysicsResults()
{
	// Get results from gScene->simulate(gDeltaTime)
	while (!gScene->fetchResults(NX_RIGID_BODY_FINISHED, false));
}

void Physixer::ReleaseActor(NxActor* actor, NxScene* scene) 
{
	if(actor) 
	{
		NxConvexMesh* convex = NULL;
		NxTriangleMesh* mesh = NULL;
		NxU32 n = actor->getNbShapes();
		if (n > 0) 
		{
			for(NxU32 i = 0; i < n; ++i)
			{
				NxShape* shape = actor->getShapes()[i];
				if (shape->isConvexMesh()) 
				{
					convex = &(shape->isConvexMesh()->getConvexMesh());
				}
				else if(shape->isTriangleMesh()) 
				{
					mesh = &(shape->isTriangleMesh()->getTriangleMesh());
				}
				actor->releaseShape(*shape);
				if (convex && convex->getReferenceCount() == 0) {
					gPhysicsSDK->releaseConvexMesh(*convex);
					convex = NULL;
				}
				if (mesh && mesh->getReferenceCount() == 0) {
					gPhysicsSDK->releaseTriangleMesh(*mesh);
					mesh = NULL;
				}
			}
		}
		scene->releaseActor(*actor);
	}
}

void Physixer::ReleaseScene(NxScene* scene)
{
	if(scene)
	{
		NxU32 n = scene->getNbActors();
		NxActor** pa = scene->getActors();
		for(NxU32 i = 0; i < n; ++i)
		{
			ReleaseActor(pa[i], scene);
		}
		gPhysicsSDK->releaseScene(*scene);
	}
}

void Physixer::ReleasePhysicsSDK(NxPhysicsSDK* pSDK)
{
	if(pSDK)
	{
		NxU32 n = pSDK->getNbScenes();
		for(NxU32 i = 0; i < n; ++i)
		{
			NxScene* s = pSDK->getScene(i);
			ReleaseScene(s);
		}
		NxReleasePhysicsSDK(pSDK);
	}
}

D3DXMATRIX	Physixer::getPartMatrix(partEnum p)
{
	D3DXMATRIX d3dmat;

	switch(p)
	{
		case HEAD: 			guy->head->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
		case TORSO: 		guy->torso->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
		case PELVIS: 		guy->pelvis->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
		case UPPER_ARM_L: 	guy->leftUpperArm->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
		case UPPER_ARM_R: 	guy->rightUpperArm->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
		case FOREARM_L: 	guy->leftForeArm->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
		case FOREARM_R: 	guy->rightForeArm->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
		case HAND_L: 		guy->leftHand->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
		case HAND_R: 		guy->rightHand->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
		case THIGH_L: 		guy->leftThigh->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
		case THIGH_R: 		guy->rightThigh->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
		case CALF_L: 		guy->leftCalf->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
		case CALF_R: 		guy->rightCalf->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
		case FOOT_L: 		guy->leftFoot->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
		case FOOT_R: 		guy->rightFoot->getGlobalPose().getColumnMajor44((float*)d3dmat); break;
	}

	return d3dmat;
}

float Physixer::UpdateTime()
{
	unsigned int currentTime = timeGetTime();
	unsigned int elapsedTime = currentTime - previousTime;
	previousTime = currentTime;
	return (float)(elapsedTime)*0.001f;
}
